import math

def rotate_point(px, py, theta):
    """
    在(0,0)点绕原点对 (px, py) 进行逆时针旋转 theta 弧度
    返回旋转后的 (x', y')
    """
    x_new = px * math.cos(theta) - py * math.sin(theta)
    y_new = px * math.sin(theta) + py * math.cos(theta)
    return x_new, y_new

def get_wheel_polygon(wheel_len, wheel_wid):
    # 以轮子中心为(0,0)，轮子朝 x 轴方向 => 顶点(±wheel_len/2, ±wheel_wid/2)
    wl = wheel_len / 2.0
    ww = wheel_wid / 2.0
    return [(-wl, -ww), (wl, -ww), (wl, ww), (-wl, ww)]

def build_wheel_patches(axle_x, center_y, angle, wheel_center_offset, wheel_len, wheel_wid):
    """
    返回左右两个轮子的多边形顶点列表(每个轮子都是4个点),
    先在局部绕自身中心旋转(angle)，再平移(axle_x, center_y ± offset)
    """
    polygons = []

    # 先获取未旋转的轮子多边形
    base_wheel = get_wheel_polygon(wheel_len, wheel_wid)

    # 左轮中心: y = center_y + wheel_center_offset
    # 右轮中心: y = center_y - wheel_center_offset
    for side in [+1, -1]:
        # 1) 绕轮子中心旋转
        rotated_pts = []
        for (x0, y0) in base_wheel:
            rx, ry = rotate_point(x0, y0, angle)
            rotated_pts.append((rx, ry))

        # 2) 平移轮心位置
        #    轮心在(axle_x, center_y + side*wheel_center_offset)
        wheel_cx = axle_x
        wheel_cy = center_y + side * wheel_center_offset
        translated_pts = [(rx + wheel_cx, ry + wheel_cy) for (rx, ry) in rotated_pts]

        polygons.append(translated_pts)

    return polygons

def rotate_and_translate(points_list, heading, tx, ty):
    new_list = []
    for (x0, y0) in points_list:
        # 先绕(0,0) 旋转 heading
        rx, ry = rotate_point(x0, y0, heading)
        # 再平移 (tx, ty)
        new_list.append((rx + tx, ry + ty))
    return new_list

def shift_points(points, dx, dy):
    return [(px-dx, py-dy) for (px, py) in points]

def polygon_center(points):
    """
    简单求多边形顶点的平均值，作为“中心”近似
    points: [(x1, y1), (x2, y2), ...]
    返回 (cx, cy)
    """
    sx = sy = 0.0
    for (x, y) in points:
        sx += x
        sy += y
    n = len(points)
    return (sx / n, sy / n)
